'''
Robot arm returns to mechanical zero point and status query
'''
from wlkata_mirobot import WlkataMirobot
import time
import re 

import socket
import threading
from queue import Queue

arm = WlkataMirobot()
arm.home()

def receive_data(ip_port, new_client):
    while True:
        data = new_client.recv(1024).decode("gbk")
        print(f"data is {data}")
        if not data:
            print(f"the data is uncreadble 2023-12 -5")
            new_client.close()
            break
        
        if data == "?":
            send_data2Client = str(arm.get_status())
            new_client.send(send_data2Client.encode("gbk"))
            break
        else:
            pattern = r'(-?\d+(?:\.\d+)?)'
            joint_angles = re.findall(pattern, data)
            arm._parse_status(data)
            # accoring to G code control kata-robot
            arm.go_to_axis(joint_angles[0], joint_angles[1], joint_angles[2], joint_angles[3], joint_angles[4], joint_angles[5], joint_angles[6], 5, False, True)
            new_data = "ok"
            new_client.send(new_data.encode("gbk"))
            break

if __name__== "__main__":
    tcp_server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    tcp_server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, True)
    tcp_server_socket.bind(("", 2438))
    tcp_server_socket.listen(128)

    while(1):
        new_client, ip_port = tcp_server_socket.accept()
        sub_thread = threading.Thread(target=receive_data, args=(ip_port, new_client))
        sub_thread.setDaemon(True)
        sub_thread.start()
